/******************************************************************************
 * Copyright 2022 The Airos Authors. All Rights Reserved.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 * http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *****************************************************************************/

#include <iostream>
#include <map>
#include <string>
#include <thread>
#include <vector>

#include "base/device_connect/lidar/device_factory.h"
#include "yaml-cpp/yaml.h"

using os::v2x::device::LidarDevice;
using os::v2x::device::LidarDeviceFactory;
using os::v2x::device::LidarPBDataTypePtr;

#define SHOW_DASH " ################## "
#define SHOW_TITLE(name) SHOW_DASH name SHOW_DASH

static void proc_lidar_data(const LidarPBDataTypePtr& data) {
  std::cout << SHOW_TITLE("LiDARData") << std::endl;
  std::cout << "header.sequence_num: " << data->header().sequence_num()
            << std::endl;
  std::cout << "frame_id: " << data->frame_id() << std::endl;
  std::cout << "is_dense: " << data->is_dense() << std::endl;
  for (const auto& pt : data->point()) {
    std::cout << "point: {"
              << "\n (" << pt.x() << "," << pt.y() << "," << pt.z() << ")"
              << "\n intensity: " << pt.intensity()
              << "\n stamp: " << pt.stamp()
              << "\n row_number: " << pt.row_number()
              << "\n column_number: " << pt.column_number() << "\n}"
              << std::endl;
  }
  std::cout << "measurement_time: " << (int)data->measurement_time()
            << std::endl;
  std::cout << "width: " << data->width() << std::endl;
  std::cout << "height: " << data->height() << std::endl;
}

bool parse_config(const std::string& ex_conf, std::string& type_name,
                  std::string& lidar_name, std::string& lidar_conf) {
  try {
    YAML::Node node = YAML::LoadFile(ex_conf);
    type_name = node["device_type"].as<std::string>();
    lidar_name = node["lidar_name"].as<std::string>();
    lidar_conf = node["lidar_config"].as<std::string>();
  } catch (const std::exception& err) {
    std::cerr << err.what() << std::endl;
    return false;
  } catch (...) {
    std::cerr << "Parse yaml config failed: " << ex_conf << std::endl;
    return false;
  }

  return true;
}

int main(int argc, char** argv) {
  if (argc < 2) {
    std::cout << "Args Error!\nUsage: lidar_test_tool conf_file1 conf_file2 ..."
              << std::endl;
    return 1;
  }

  std::map<std::string, std::shared_ptr<LidarDevice>> device_name_map;
  std::string device_type, lidar_name, lidar_conf;
  for (int i = 1; i < argc; ++i) {
    if (!parse_config(argv[i], device_type, lidar_name, lidar_conf)) {
      std::cerr << "Parse conf_file failed! skip: " << argv[i] << std::endl;
      continue;
    }

    auto device =
        LidarDeviceFactory::Instance().GetShared(device_type, proc_lidar_data);
    if (!device) {
      std::cerr << "Build device instance failed! skip: " << device_type
                << std::endl;
      continue;
    }
    if (!device->Init(lidar_conf)) {
      std::cerr << "Init " << lidar_name << " failed! skip: " << lidar_name
                << std::endl;
      continue;
    }
    device_name_map[lidar_name] = device;
  }

  std::map<std::string, std::unique_ptr<std::thread>> task_map;
  for (const auto& item : device_name_map) {
    lidar_name = item.first;
    auto& device = item.second;
    task_map.emplace(lidar_name,
                     new std::thread([device]() { device->Start(); }));
  }

  for (auto& item : task_map) {
    auto& task = item.second;
    if (task->joinable()) {
      task->join();
    }
  }

  return 0;
}
